/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* Converts a log file provi/ded by Rocla people to standard form.
*/

#include<stdio.h>
#include<stdlib.h>
#include<math.h>

#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "CParticleFilter.h"

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
/// Read Scan measurement file with arbitrary number of items (and allowing white space before '\n')
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
int forwardString(const char *buf, int begin,int items){
		int i=begin+1;
		int cnt = 0;
		while(cnt<items){
				if(buf[i]==' ') cnt++;
				i++;
		}
		return i;
}

/// For reading the data from file
int readScanLine(FILE *f,Grid::TScan &meas, Grid::pose &p, Grid::pose &prev){
		float time_s;
		int i=0;
		char in_line[16096];
		char *ret;
		char c=0;
		static float old_distance = 0;
		ret = fgets (in_line, 16096, f );
		if(ret==NULL){
				if(feof(f)){
						fprintf(stderr,"Reached end of file -- Alles klar!\n");
						return 0;
				}else{
						fprintf(stderr,"Some error occurred...sorry contact Jari :)\n");
						return 1;
				}
		}
		int cnt=0;
		while( (c!='\n')){
				if(in_line[i]==' ') cnt++;
				else if(in_line[i]=='\n') break;
				i++;
		}
		//time[s] distance[mm] angle[deg] r0[cm] a0[deg] r1 a1 ...  rn an
		//fprintf(stderr,"There is %d floats in this file (= %d meas points)\n",cnt,(cnt-3)/2);
		//sscanf(in_line,"%f %f %f ",&p.x,&p.y,&p.a); ///< Pose header
		float d,phii;
		sscanf(in_line,"%f %f %f",&time_s,&d,&phii); ///< Pose header
		d /= 1000.0;
		phii /= 1000.0;
		phii *= M_PI/180.0;  
		
		float dd = d-old_distance;
		old_distance = d;
		
	
		p.x = dd * cos(prev.a + phii) + prev.x;
		p.y = dd * sin(prev.a + phii) + prev.y;
		p.a = phii; 
		//p.a -= M_PI/2.0;
		
		p.toPI();
		int pointer_ind=0;
		pointer_ind = forwardString(in_line,0,3); ///search the next begin for float
		meas.N = (cnt-3)/2;
		i =0;
		while(i<((cnt-3)/2)){
				sscanf((in_line+pointer_ind),"%f %f",&meas.r[i],&meas.a[i]);
				meas.a[i] *= M_PI/180.0;
				meas.a[i] -=  M_PI/2.0;
				meas.r[i] /= 100.0;
				if(meas.r[i]<=0.5) meas.r[i]=0;  
				pointer_ind = forwardString(in_line,pointer_ind,2); ///search the next begin for float
				i++;
		}
		
		return 0;
}

void printUsage(){
		fprintf(stderr,"\nConverts Rocla provided log files into a 'standard' format\n\n");
		fprintf(stderr,"\t Usage: ./rocla_convert [log_file] [out_file]\n");
}

int main(int argc, char *argv[]){
		const char *measurement_file_name="pitka2.txt";
		const char *out_file_name = "rocla_converted.txt";
		int enough_scan_poinsts = 800; ///< enough to reserve memory for the scan points in the meas file.
		
		printUsage();
		
		if(argc >= 2) measurement_file_name = argv[1];
		if(argc >= 3) out_file_name = argv[2];
		
		Grid::TScan meas;
		meas.alloc(enough_scan_poinsts);
		Grid::pose pos,prev;
		
		FILE *log = fopen(measurement_file_name,"rt"); ///< data file
		if(log == NULL){
				fprintf(stderr,"Unable to open the log file\n");
				exit(1);
		}
		FILE *fout = fopen(out_file_name, "wt");
		
		int cnt = 0;
		readScanLine(log,meas,pos, prev);
		fprintf(stderr,"Meas.N='%d' a[0]=%.2f a[n]=%.2f\n",
						meas.N,meas.a[0]*180.0/M_PI,meas.a[meas.N-1]*180.0/M_PI);
		pos.set(0,0,0);
		while(!feof(log)){
				readScanLine(log,meas,pos,prev);
				prev = pos;
				
				//fprintf(stderr,"%.2f %.2f %.2f\n",pos.x,pos.y,pos.a);
				fprintf(fout,"%.3f %.3f %.3f ",pos.x,pos.y,pos.a);
				for(int i=0;i<meas.N;i++){
						fprintf(fout," %.2f %.2f",meas.r[i],meas.a[i]);
				}
				fprintf(fout,"\n");
				
		}
		
		
		
		fclose(fout);
		return 0;
}
		
		
		
		
